# python实现四元数、欧拉角、旋转矩阵、旋转向量的相互转换
# https://blog.csdn.net/cg1135217680/article/details/130105937

from scipy.spatial.transform import Rotation as R
import cv2
import numpy as np
import math

def rotvector2rot(rotvector):
    """旋转向量转旋转矩阵"""
    Rm = cv2.Rodrigues(rotvector)[0]
    return Rm

def quaternion2euler(quaternion):
    """四元数转欧拉角"""
    r = R.from_quat(quaternion)
    euler = r.as_euler('xyz', degrees=True)
    return euler

def euler2quaternion(euler):
    """.欧拉角转四元数"""
    r = R.from_euler('xyz', euler, degrees=True)
    quaternion = r.as_quat()
    return quaternion

def euler2rot(euler):
    """欧拉角转旋转矩阵"""
    r = R.from_euler('xyz', euler, degrees=True)
    # rotation_matrix = r.as_matrix()  # scipy 1.7.0以上
    rotation_matrix = r.as_dcm()
    return rotation_matrix

def isRotationMatrix(R):
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype=R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6

def rot2euler(R):
    """旋转矩阵转欧拉角"""
    assert (isRotationMatrix(R))

    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    singular = sy < 1e-6

    if not singular:
        x = math.atan2(R[2, 1], R[2, 2]) * 180 / np.pi
        y = math.atan2(-R[2, 0], sy) * 180 / np.pi
        z = math.atan2(R[1, 0], R[0, 0]) * 180 / np.pi
    else:
        x = math.atan2(-R[1, 2], R[1, 1]) * 180 / np.pi
        y = math.atan2(-R[2, 0], sy) * 180 / np.pi
        z = 0

    return np.array([x, y, z])

def quaternion2rot(quaternion):
    """四元数转旋转矩阵"""
    r = R.from_quat(quaternion)
    rot = r.as_matrix()
    return rot


if __name__ == '__main__':
    # # 旋转向量转旋转矩阵
    # rotvector = np.array([[0.223680285784755, 0.240347886848190, 0.176566110650535]])
    # print(rotvector2rot(rotvector))
    # # 输出
    # # [[ 0.95604131 -0.14593404  0.2543389 ]
    # #  [ 0.19907538  0.95986385 -0.19756111]
    # #  [-0.21529982  0.23950919  0.94672136]]
    #
    # # 四元数转欧拉角
    # quaternion = [0.03551, 0.21960, -0.96928, 0.10494]
    # print(quaternion2euler(quaternion))
    # # 输出
    # # [ -24.90053735    6.599459   -169.1003646 ]
    #
    # # 欧拉角转四元数
    # euler = [-24.90053735, 6.599459, -169.1003646]
    # print(euler2quaternion(euler))
    # # 输出
    # # [ 0.03550998  0.21959986 -0.9692794   0.10493993]
    #
    # # 欧拉角转旋转矩阵
    # euler = [-24.90053735, 6.599459, -169.1003646]
    # print(euler2rot(euler))
    # # 输出
    # # [[-0.9754533   0.21902821 -0.02274859]
    # #  [-0.18783626 -0.88152702 -0.43316008]
    # #  [-0.11492777 -0.41825442  0.90102988]]
    #
    # # 旋转矩阵转欧拉角
    #
    # rot = np.array([[-1.01749712e-02, 9.99670705e-01, -2.35574076e-02],
    #                 [-9.99890780e-01, -1.04241019e-02, -1.04769347e-02],
    #                 [-1.07190495e-02, 2.34482322e-02, 9.99667586e-01]])
    #
    # print(rot2euler(rot))
    # # 输出
    # # [  1.34368509   0.61416806 -90.58302646]
    #
    #
    # # 四元数转旋转矩阵
    # quaternion = [0.03551, 0.21960, -0.96928, 0.10494]
    # print(quaternion2rot(quaternion))
    # # 输出
    # # [[-0.9754533   0.21902821 -0.02274859]
    # #  [-0.18783626 -0.88152702 -0.43316008]
    # #  [-0.11492777 -0.41825442  0.90102988]]


    # quaternion = [-0.007202,0.708623,-0.705546,-0.002350]
    # print(quaternion2rot(quaternion))

    # t = np.array([89.38200710464973, -0.30336329523187511, -3.0148378795424207])/1000
    # eul = [0.00074892604071712901, 0.03276349707996086, -0.015098076869030244]
    # rot = euler2rot(eul)
    # T = np.insert(rot, 3, values=t, axis=1)
    # for line in T:
    #     for n in line:
    #         print(n, end=',')
    #     print()
    # print("0,0,0,1")


    euler = [0. , 0. , 90.]
    print(euler2quaternion(euler))